from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, ExecuteProcess
from launch.conditions import IfCondition
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration, PythonExpression
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory

import os

def generate_launch_description():
    # 获取包路径
    bringup_pkg_share = get_package_share_directory('ball_collector_bringup')
    
    # 声明启动参数
    demo_scenario = LaunchConfiguration('demo_scenario')
    enable_recording = LaunchConfiguration('enable_recording')
    
    # 参数声明
    declare_demo_scenario = DeclareLaunchArgument(
        'demo_scenario',
        default_value='basic',
        description='Demo scenario to run (basic, multi_color, time_trial)'
    )
    
    declare_enable_recording = DeclareLaunchArgument(
        'enable_recording',
        default_value='false',
        description='Whether to record the demonstration'
    )
    
    # 包含系统启动文件
    system_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(bringup_pkg_share, 'launch', 'system_bringup.launch.py')
        ),
        launch_arguments={
            'use_simulation': 'true',  # 示例中使用模拟
            'use_rviz': 'true',
            'use_yolo': 'true',
            'robot_name': 'demo_robot'
        }.items()
    )
    
    # 演示配置节点
    demo_config_node = Node(
        package='ball_collector',
        executable='demo_config_node',  # 假设存在这样的节点
        name='demo_config_node',
        namespace='demo_robot',
        parameters=[{
            'scenario': demo_scenario,
            'start_delay': 5.0,
            'target_colors': ['red', 'blue', 'green'],
            'time_limit': 120.0
        }]
    )
    
    # 演示控制器节点
    demo_controller_node = Node(
        package='ball_collector',
        executable='demo_controller_node',  # 假设存在这样的节点
        name='demo_controller_node',
        namespace='demo_robot',
        parameters=[{
            'scenario': demo_scenario,
            'auto_start': True,
            'difficulty': 'medium'
        }]
    )
    
    # 添加性能监控节点
    performance_monitor_node = Node(
        package='ball_collector',
        executable='performance_monitor_node',  # 假设存在这样的节点
        name='performance_monitor_node',
        namespace='demo_robot',
        parameters=[{
            'metrics': ['collection_rate', 'avg_collection_time', 'success_rate'],
            'log_to_file': True,
            'log_directory': '/tmp/ball_collector_demo'
        }]
    )
    
    # 记录所有话题（如果启用）
    record_process = ExecuteProcess(
        cmd=['ros2', 'bag', 'record', '-a', '-o', '/tmp/ball_collector_demo'],
        output='screen',
        condition=IfCondition(enable_recording)
    )

    # 创建启动描述
    ld = LaunchDescription()
    # 添加参数声明
    ld.add_action(declare_demo_scenario)
    ld.add_action(declare_enable_recording)
    # 添加系统启动
    ld.add_action(system_launch)
    # 添加演示特定节点
    ld.add_action(demo_config_node)
    ld.add_action(demo_controller_node)
    ld.add_action(performance_monitor_node)
    ld.add_action(record_process)
    
    return ld